import threading, Queue
import time
import sys,os

# find the mavlink.py module
for d in [ 'pymavlink',
           os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'MAVLink' ,'pymavlink') ]:
    if os.path.exists(d):
        sys.path.insert(0, d)
        if os.name == 'nt':
            try:
                # broken python compilation of mavlink.py on windows!
                os.unlink(os.path.join(d, 'mavlinkv10.pyc'))
            except:
                pass

import mavlinkv10 as mavlink
import mavutil


class mavlink_vario_processes:
    def __init__(self, vario_callback):
        self.receiver       = None
        self.vario_callback = vario_callback

    def shutdown_hook(self, t_id, child):
        print('%s - Unexpected thread shutdown, handle this.. restart thread?' % str(t_id))
        
    def splot(self, device, baud):
        print('It got there')
        

    def start_mavlink_services(self, device, baud, master, system, component):
        try:
            self.MAVServices
        except:
            print("MAVlink service does not exist")
        else:
            print("Checking MAVlink services are running")
            if self.MAVServices.isAlive():
                print("MAVlink services running. waiting for stop")
                self.MAVServices.stop_services()
                self.MAVServices.join(1)
                if self.MAVServices.isAlive():
                    print("MAVlink services already running. stop services first")
                    return

        print("Creating services")
        self.MAVServices = MAVlink_services(device, baud, master, system, component, self.vario_callback)  #shutdown_hook = self.shutdown_hook
        self.MAVServices.start()

    def stop_services(self):
        try:
            self.MAVServices
        except:
            return
        else:
            self.MAVServices.stop()

    def services_running(self):
        try:
            self.MAVServices
        except:
            return False
        else:
            return self.MAVServices.isAlive()


    def tx_msg_append(self, tx_msg):
        try:
            self.MAVServices
        except:
            print("MAV services thread does not exist, can not add message to tx queue")
            return False
        else:
            return self.MAVServices.tx_msg_append(tx_msg)


class MAVlink_services(threading.Thread):
    def __init__(self, device, baud, master, system, component, vario_callback):
        threading.Thread.__init__(self)

        self._stop = threading.Event()
        self.device = device
        self.baud = baud
        self.master = master
        self.system = system
        self.component = component
        
        self.vario_callback = vario_callback

        self.tx_q       = Queue.Queue(3)
        
        self.heartbeat_timer = 0
        self.heartbeat_ok = False

        comment = "starting connection " + self.device + " baud {:d} , as master{:d}, with system{:d}".format(self.baud, self.master, self.system)
        print(comment)
        self.mav_fd = mavutil.mavlink_connection(self.device, self.baud, self.master)

        self.MAVrx = MAVlink_receiver(self.mav_fd)

    def stop(self):
        print("MAVlink service thread request stop")
        self._stop.set()

    def stopped(self):
        return not self.isAlive()

    def tx_msg_append(self, mesg):
        if self.isAlive():
            try:
                self.tx_q.put(mesg)
            except:
                print("transmit buffer full")
            else:
                print("message added to queue")
                return True
        else:
            print("MAVlink services not alive, can not put message in queue")
        return False


    def run(self):
        self._stop.clear()
        print("MAVlink service thread starting")

        self.MAVrx.start()

        while(not self._stop.isSet() ):
            try:
                msg = self.MAVrx.rx_q.get(True, 0.1)
            except Queue.Empty:
                pass
            else:
                if msg and msg.get_type() == "HEARTBEAT":
                    print(msg)        
                    system = msg.get_srcSystem()
                    component = msg.get_srcComponent()
                    print("Heartbeat from UDB (system %u component %u)" % (system, component))
                    if((system == self.system) and (component == self.component)):
                        self.heartbeat_ok = True
                        self.heartbeat_timer = time.time()

                if msg and msg.get_type() == "GLOBAL_POSITION_INT":
                    try:
                        vz = msg.vz   # vertical velocity in cm/s
                        vz = int(-vz)
                    except:
                        print("decode global vz message fail")
                        continue
                    try:
                        self.vario_callback(vz)
                    except:
                        print("vario callback fail")

                self.MAVrx.rx_q.task_done()

            if(time.time() > (self.heartbeat_timer + 1) ):
                self.heartbeat_ok = False
                self.vario_callback(0)


        self.MAVrx.stop()
        self.MAVrx.join(5)

        self.mav_fd.port.close()
        self.mav_fd = None
        
        try:
            self.vario_callback(0)      # clear the vario
        except:
            print("vario callback fail")
     
        print("MAVlink service thread terminated")



class MAVlink_receiver(threading.Thread):
    def __init__(self, mav_fd):
        threading.Thread.__init__(self)

        self._stop = threading.Event()
        self.mav_fd = mav_fd
        self.mav_fd.message_hooks.append(self.rx_callback)

        self.rx_q       = Queue.Queue(20)
        
    def rx_callback(self, mav_fd, msg):
        if(msg != None):
            if(self.rx_q.full()):
                try:
                    self.rx_q.get_nowait()
                    self.rx_q.task_done()
                except:
                    pass
            try:
                self.rx_q.put_nowait(msg)
            except:
                pass

    def stop(self):
        print("MAVlink receive thread request stop")
        self._stop.set()

    def stopped(self):
        return not self.isAlive()                

    def run(self):
        self._stop.clear()
        print("MAVlink receive thread starting")

#        while(not self._stop.isSet() ):
#            try:
#                s = self.mav_fd.recv()
                
        while(not self._stop.isSet() ):
            msg = self.mav_fd.recv_msg()
                            
        print("MAVlink receive thread terminated")
        #=======================================================================
        #    except:
        #        self._stop.wait(0.01)
        #    else:
        #        if len(s) == 0:
        #            self._stop.wait(0.01)
        #            continue
        #        for c in s:
        #            msg = self.mav_fd.mav.parse_char(c)
        #            if not msg:
        #                continue
        # #            if msg.get_type() == "BAD_DATA":
        # #                if all_printable(msg.data):
        # #                    sys.stdout.write(msg.data)
        # #                    sys.stdout.flush()
        #            else:
        #                if(self.rx_q.full()):
        #                    try:
        #                        self.rx_q.get_nowait()
        #                    except:
        #                        continue
        #                    else:
        #                        self.rx_q.task_done()
        #                try:
        #                    self.rx_q.put_nowait(msg)
        #                except:
        #                    continue
        #=======================================================================
                        
        

